#!/usr/bin/env python
#coding=utf-8
import rospy
import cv2
from aikit_lib import aikit_2wd
aikit=aikit_2wd.robot() 
p1=[1.76,1.58,90]
shibiequ_info=[0.78,3.15,90]

#两个食物柜子的坐标
food_cabinet1=[2.65,3.09,90]
food_cabinet2=[3.15,2.9,-90]
#两个水果柜子的坐标
fruit_cabinet1=[2.40,0.20,-90]
fruit_cabinet2=[3.00,0.15,90]
#四个椅子柜子的坐标
chair_cabinet1=[2.39,2.03,90]
# chair_cabinet2=[2.39,2.03,90]
# chair_cabinet3=[2.39,2.03,90]
# chair_cabinet4=[2.39,2.03,90]

#八个物品应该放置的导航坐标
hamburger_info=[0.2,2.31,0]
bottle_info=[0.11,1.53,0]
apple_info=[1.52,2.02,180]
pitaya_info=[1.82,1.38,180]

chair1_info=[1.46,1.51,180]
chair2_info=[0,1.99,0]

object_dict={
    "shibiequ":shibiequ_info,
    "food1":food_cabinet1,
    "food2":food_cabinet2,
    "fruit1":fruit_cabinet1,
    "fruit2":fruit_cabinet2,  
    "hamburger":hamburger_info,
    "bottle":bottle_info,
    "apple":apple_info,
    "pitaya":pitaya_info,
    "chair1":chair1_info,
    "chair1":chair_cabinet1,
    "chair2":chair2_info}

#不同物品设置不同的夹子夹持角度，避免舵机过热保护，丢掉物品
claw_dict={"bottle":0,"hamburger":15,"apple":8,"pitaya":10}
name_list=["bottle","hamburger","apple","pitaya"]

def nav_to_name(name_string):
    aikit.nav_to(object_dict[name_string][0],object_dict[name_string][1],object_dict[name_string][2])
#夹子夹物品函数，输入物品名称即可
def claw_name(name_string):
    aikit.set_claw(claw_dict[name_string])

#瞄准目标函数，根据画面的偏差，经过比例系数kt变成转向的时间，来瞄准（笨办法，误差较大）
def focus_object():
    camera_middle_pos=297
    kt=0.013
    b=0
    while True:
        roi_data=aikit.objects_data
        if len(roi_data)>0:
            x_middle_pos=roi_data[0].x+roi_data[0].width
            x_error=x_middle_pos-camera_middle_pos
            print("x_error",x_error)
            t=kt*abs(x_error)
            if t>2:
                t=2
            print("time",t)
            if x_error<0:
                aikit.turn(0.3,t)
                print("focus done")
                break
            if x_error>0:
                aikit.turn(-0.3,t)
                break
#靠近目标函数，输入参数为需要和前面墙保持的距离，默认参数为0.25m
def close_to_object(distance=0.25):
    while(aikit.lidar_range()-distance>0):
        aikit.set_speed(0.2,0)
    aikit.set_speed(0,0)

#[label,confidence,x,y,width,height]
def object_detect():
    while True:
        objects_data=aikit.objects_data
        if len(objects_data)==1:
            return objects_data[0].label
        if len(objects_data)==2:
            if objects_data[0].y>objects_data[1].y:
                return objects_data[0].label
            else:
                return objects_data[1].label
        if len(objects_data)>2:
            return objects_data[0].label

def do_task(cabinet_name):
    aikit.set_claw(100)
    nav_to_name(cabinet_name)
    close_to_object(0.6)
    focus_object()  
    result=object_detect()
    if result in name_list:
        aikit.play_sound("识别结果为")
        aikit.play_sound(str(result))
        aikit.play_sound("开始抓取")
        close_to_object()
        claw_name(result)
        aikit.play_sound("抓取完成")
        aikit.go(-0.3,2)
        nav_to_name(result)
        close_to_object(0.18)
        aikit.set_claw(60)
        aikit.go(-0.3,3)

if __name__=="__main__":
    aikit.set_claw(100)
    aikit.set_pose()
    aikit.play_sound("餐桌布置任务开始")
    do_task("fruit1")
    do_task("fruit2")
    # do_task("food2")
    # do_task("food1")

# #单个抓取测试
# ###################################################
#     aikit.set_claw(100)
#     close_to_object(0.6)
#     focus_object()  
#     result=object_detect()
#     print(result)
#     if result in name_list:
#         aikit.play_sound("识别结果为")
#         aikit.play_sound(str(result))
#         aikit.play_sound("开始抓取")
#         close_to_object()
#         claw_name(result)
#         aikit.play_sound("抓取完成")
#         aikit.go(-0.3,3)
# #####################################################





